from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator()  # 节点
    nav.waitUntilNav2Active()  # 等待导航可用，及等待激活

    goal_poses = []
    goal_pose1 = PoseStamped()
    goal_pose1.header.frame_id = "map"
    goal_pose1.header.stamp = nav.get_clock().now().to_msg()
    goal_pose1.pose.position.x = 2.0
    goal_pose1.pose.position.y = 1.0
    goal_pose1.pose.orientation.w = 1.0
    goal_poses.append(goal_pose1)

    goal_pose2 = PoseStamped()
    goal_pose2.header.frame_id = "map"
    goal_pose2.header.stamp = nav.get_clock().now().to_msg()
    goal_pose2.pose.position.x = 3.0
    goal_pose2.pose.position.y = -1.0
    goal_pose2.pose.orientation.w = 1.0
    goal_poses.append(goal_pose2)

    goal_pose3 = PoseStamped()
    goal_pose3.header.frame_id = "map"
    goal_pose3.header.stamp = nav.get_clock().now().to_msg()
    goal_pose3.pose.position.x = 0.0
    goal_pose3.pose.position.y = 0.0
    goal_pose3.pose.orientation.w = 1.0
    goal_poses.append(goal_pose3)


    # nav.goToPose(goal_pose1)  # 这个会进行spin
    nav.followWaypoints(goal_poses)
    while not nav.isTaskComplete():
        feedback = nav.getFeedback()
        nav.get_logger().info(f"当前路点编号:{feedback.current_waypoint}")
        # nav.cancelTask 超时取消
    result = nav.getResult()
    nav.get_logger().info(f"导航结果：{result}")

    # rclpy.spin(nav)
    # rclpy.shutdown()